package kit.edu.lego.gruppe3.sensors;

import kit.edu.lego.gruppe3.ComponentsRepository;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.UltrasonicSensor;
import lejos.util.Delay;

public class Scanner {

	private static NXTRegulatedMotor rotor = ComponentsRepository.getInstance()
			.getMiddleMotor();
	private static UltrasonicSensor sonicS = ComponentsRepository.getInstance()
			.getSonicSensor();
	private static LightSensor lightS = ComponentsRepository.getInstance()
			.getLightSensor();
	private static GroundType[] grounds = new GroundType[37];
	private static int[] ranges = new int[19];
	private static int[] angles = new int[19];
	private static int offset;
	private static final int[] thresholds = { 275, 350, 460, 600 };
	public static boolean stop = false;

	public static void setup(int off) {
		offset = off;
		int k = 0;
		for (int i = offset - 90; i <= offset + 90; i += 10) {
			angles[k] = i;
			k++;
		}
	}

	public static boolean stop() {
		boolean result = !stop;
		stop = true;
		return result;
	}

	public static boolean unstop() {
		boolean result = stop;
		stop = false;
		return result;
	}

	public static void scan() {
		if (Math.abs(rotor.getPosition() - angles[0]) < Math.abs(rotor
				.getPosition()
				- angles[18])) {
			for (int i = 0; i <= 18; i++) {
				scanAt(i * 10 - 90);
			}
		} else {
			for (int i = 0; i <= 18; i++) {
				scanAt(90 - 10 * i);
			}

		}
	}

	public static void scanAt(int angle) {
		int i = (angle + 90) / 10;
		rotor.rotateTo(angles[i]);
		ranges[i] = sonicS.getDistance();
		int t = lightS.getNormalizedLightValue();
		if (t < thresholds[0]) {
			grounds[i] = GroundType.Air;
		} else if (t < thresholds[1]) {
			grounds[i] = GroundType.Black;
		} else if (t < thresholds[2]) {
			grounds[i] = GroundType.Wood;
		} else if (t < thresholds[3]) {
			grounds[i] = GroundType.Line;
		} else {
			grounds[i] = GroundType.BridgeLight;
		}
	}

	public static GroundType[] getGrounds() {
		return grounds;
	}

	public static GroundType[] getGrounds(boolean rescan) {
		if (rescan)
			scan();
		return grounds;
	}

	public static GroundType getGroundTypeAt(int angle) {
		return getGroundTypeAt(angle, false);
	}

	public static GroundType getGroundTypeAt(int angle, boolean rescan) {
		if (angle % 10 != 0 || angle < -90 || angle > 90)
			return GroundType.Error;
		if (rescan)
			scanAt(angle);
		return grounds[(angle + 90) / 10];
	}

	public static int[] getRanges() {
		return ranges;
	}

	public static int[] getRanges(boolean rescan) {
		if (rescan)
			scan();
		return ranges;
	}

	public static int getRangeAt(int angle) {
		return getRangeAt(angle, false);
	}

	public static int getRangeAt(int angle, boolean rescan) {
		if (angle % 10 != 0 || angle < -90 || angle > 90)
			return -1;
		if (rescan)
			scanAt(angle);
		return ranges[(angle + 90) / 10];
	}
	
	public static int getFirstLine() {
		return getFirstLine(false);
	}
	public static int getFirstLine(boolean rescan) {
		return getFirstOf(GroundType.Line, rescan);
	}
	public static int getFirstOf(GroundType type) {
		return getFirstOf( type, false);
	}

	public static int getFirstOf(GroundType type, boolean rescan) {
		if (rescan)
			scan();
		int i = 0;
		int k = 0;
		while (i < 19 && grounds[i] != type)
			i++;
		if (i < 19) {
			while (i+k < 19 && grounds[i+k] == type)
				k++;
			return 10 * Math.round(i + k / 2) - 90;
		} else
			return -255;
	}

	public static int getLastLine() {
		return getLastLine(false);
	}

	public static int getLastLine(boolean rescan) {
		return getLastOf( GroundType.Line, rescan);
	}
	public static int getLastOf(GroundType type) {
		return getLastOf( type, false);
	}
	public static int getLastOf(GroundType type, boolean rescan) {
		if (rescan)
			scan();
		int i = 18;
		int k = 0;
		while (i >= 0 && grounds[i] != type)
			i--;
		if (i >= 0) {
			while (i-k >= 0 && grounds[i-k] == type)
				k++;
			return 10 * Math.round (i - k / 2) - 90;
		} else
			return -255;
	}

	public static boolean stopped() {
		return stop;
	}

	public static void scanTest() {
		while (true) {
			scanAt(0);
			LCD.clear();
			LCD.drawString("raw: " + lightS.getNormalizedLightValue(), 1,
					LCD.DISPLAY_CHAR_DEPTH - 2);
			LCD.drawString("Ground: " + getGroundTypeAt(0), 1,
					LCD.DISPLAY_CHAR_DEPTH - 1);
			LCD.drawString("Distance: " + getRangeAt(0), 0, 1);
			Delay.msDelay(100);
		}
	}
}
